/*
 * 础石Hypervisor -- 设备树解析库函数
 *
 * Copyright (C) 2022 国科础石(重庆)软件有限公司
 *
 * 作者: Xueming Dong <dongxueming@kernelsoft.com>
 *       Jingchen Zhong <zhongjingchen@kernelsoft.com>
 *       Hengxing Wu <wuhengxing@kernelsoft.com>
 *
 * License terms: GNU General Public License (GPL) version 3
 *
 */

#include <dim-sum/errno.h>
#include <dim-sum/string.h>
#include <dim-sum/libfdt.h>
#include <asm/div64.h>

#define LIBFDT_DATA32(ptr)	be32_to_cpu(*((u32 *)ptr))
#define LIBFDT_DATA64(ptr)	be64_to_cpu(*((u64 *)ptr))

/**
 * 从fdt内存地址中解析得到fdt_fileinfo
 */
int libfdt_parse_fileinfo(void *fdt_addr, struct fdt_fileinfo *fdt)
{
	struct fdt_reserve_entry *rsv;

	if (!fdt_addr || !fdt)
		return -EINVAL;

	memcpy(&fdt->header, (void *)fdt_addr, sizeof(fdt->header));
	fdt->header.magic = LIBFDT_DATA32(&fdt->header.magic);
	fdt->header.totalsize = LIBFDT_DATA32(&fdt->header.totalsize);
	fdt->header.off_dt_struct = LIBFDT_DATA32(&fdt->header.off_dt_struct);
	fdt->header.off_dt_strings = LIBFDT_DATA32(&fdt->header.off_dt_strings);
	fdt->header.off_mem_rsvmap = LIBFDT_DATA32(&fdt->header.off_mem_rsvmap);
	fdt->header.version = LIBFDT_DATA32(&fdt->header.version);
	fdt->header.last_comp_version = LIBFDT_DATA32(&fdt->header.last_comp_version);
	fdt->header.boot_cpuid_phys = LIBFDT_DATA32(&fdt->header.boot_cpuid_phys);
	fdt->header.size_dt_strings = LIBFDT_DATA32(&fdt->header.size_dt_strings);
	fdt->header.size_dt_struct = LIBFDT_DATA32(&fdt->header.size_dt_struct);

	if (fdt->header.magic != FDT_MAGIC)
		return -EINVAL;

	fdt->data = (char *)fdt_addr + fdt->header.off_dt_struct;
	fdt->data_size = fdt->header.size_dt_struct;

	fdt->str = (char *)fdt_addr + fdt->header.off_dt_strings;
	fdt->str_size = fdt->header.size_dt_strings;

	fdt->mem_rsvmap = (char *)fdt_addr + fdt->header.off_mem_rsvmap;
	fdt->mem_rsvcnt = 0;
	rsv = (struct fdt_reserve_entry *)fdt->mem_rsvmap;
	while (rsv && rsv->size) {
		fdt->mem_rsvcnt++;
		rsv++;
	}

	return 0;
}

static void libfdt_populate_devtree_recursive(struct fdt_fileinfo *fdt,
					   struct device_node *node,
					   char **data)
{
	const char *name;
	u32 len;
	struct device_node *child;

	if (!fdt || !node)
		return;

	while (LIBFDT_DATA32(*data) != FDT_END_NODE) {
		switch (LIBFDT_DATA32(*data)) {
		case FDT_PROP:
			*data += sizeof(fdt_cell_t);
			len = LIBFDT_DATA32(*data);
			*data += sizeof(fdt_cell_t);
			name = &fdt->str[LIBFDT_DATA32(*data)];
			*data += sizeof(fdt_cell_t);
			dt_set_property(node, name, *data, len);
			*data += len;
			while ((u64) (*data) % sizeof(fdt_cell_t) != 0)
				(*data)++;
			break;
		case FDT_NOP:
			*data += sizeof(fdt_cell_t);
			break;
		case FDT_BEGIN_NODE:
			*data += sizeof(fdt_cell_t);
			child = dt_node_add(node, *data);
			*data += strlen(*data) + 1;
			while ((u64) (*data) % sizeof(fdt_cell_t) != 0)
				(*data)++;
			libfdt_populate_devtree_recursive(fdt, child, data);
			break;
		default:
			return;
			break;
		};
	}

	*data += sizeof(fdt_cell_t);
}

static struct fdt_node_header *libfdt_find_matching_node_recursive(
			char **data, char *str, int level,
			int (*match)(struct fdt_node_header *, int, void *),
			void *priv)
{
	u32 len = 0x0;
	struct fdt_node_header *ret = NULL;

	if (LIBFDT_DATA32(*data) != FDT_BEGIN_NODE)
		return NULL;

	if (match((struct fdt_node_header *)(*data), level, priv))
		return (struct fdt_node_header *)(*data);

	*data += sizeof(fdt_cell_t);
	len = strlen(*data);

	*data += len + 1;
	while ((u64) (*data) % sizeof(fdt_cell_t) != 0)
		(*data)++;

	while (LIBFDT_DATA32(*data) != FDT_END_NODE) {
		switch (LIBFDT_DATA32(*data)) {
		case FDT_PROP:
			*data += sizeof(fdt_cell_t);
			len = LIBFDT_DATA32(*data);
			*data += sizeof(fdt_cell_t);
			*data += sizeof(fdt_cell_t);
			*data += len;
			while ((u64) (*data) % sizeof(fdt_cell_t) != 0)
				(*data)++;
			break;
		case FDT_NOP:
			*data += sizeof(fdt_cell_t);
			break;
		case FDT_BEGIN_NODE:
			ret = libfdt_find_matching_node_recursive(data, str,
						level + 1, match, priv);
			if (ret)
				return ret;
			break;
		default:
			return NULL;
			break;
		};
	}

	*data += sizeof(fdt_cell_t);

	return NULL;
}

struct fdt_node_header *libfdt_find_matching_node(struct fdt_fileinfo *fdt,
			int (*match)(struct fdt_node_header *, int, void *),
			void *priv)
{
	char *data = NULL;

	if (!fdt || !match)
		return NULL;

	data = fdt->data;
	return libfdt_find_matching_node_recursive(&data, fdt->str, 0, match, priv);
}

static struct fdt_node_header *libfdt_find_node_recursive(char **data,
							  char *str,
							  const char *node_path)
{
	u32 i, valid, len = 0x0;
	struct fdt_node_header *ret = NULL;

	while ((*node_path == ' ') ||
	       (*node_path == '\t') ||
	       (*node_path == '\r') ||
	       (*node_path == '\n')) {
		node_path++;
	}

	if (LIBFDT_DATA32(*data) != FDT_BEGIN_NODE)
		return NULL;

	*data += sizeof(fdt_cell_t);

	len = strlen(*data);
	valid = 1;
	for (i = 0; i < len; i++) {
		if (!node_path[i]) {
			valid = 0;
			break;
		}
		if ((*data)[i] != node_path[i]) {
			valid = 0;
			break;
		}
	}
	if (valid) {
		node_path += len;

		if (*node_path == '/')
			node_path++;

		while ((*node_path == ' ') ||
		       (*node_path == '\t') ||
		       (*node_path == '\r') ||
		       (*node_path == '\n')) {
			node_path++;
		}

		if (*node_path == '\0') {
			*data -= sizeof(fdt_cell_t);
			return (struct fdt_node_header *)(*data);
		}

		*data += len + 1;
		while ((u64) (*data) % sizeof(fdt_cell_t) != 0)
			(*data)++;

		while (LIBFDT_DATA32(*data) != FDT_END_NODE) {
			switch (LIBFDT_DATA32(*data)) {
			case FDT_PROP:
				*data += sizeof(fdt_cell_t);
				len = LIBFDT_DATA32(*data);
				*data += sizeof(fdt_cell_t);
				*data += sizeof(fdt_cell_t);
				*data += len;
				while ((u64) (*data) % sizeof(fdt_cell_t) != 0)
					(*data)++;
				break;
			case FDT_NOP:
				*data += sizeof(fdt_cell_t);
				break;
			case FDT_BEGIN_NODE:
				ret = libfdt_find_node_recursive(data, str, node_path);
				if (ret)
					return ret;
				break;
			default:
				return NULL;
				break;
			};
		}

		*data += sizeof(fdt_cell_t);
	} else {
		*data += len + 1;
		while ((u64) (*data) % sizeof(fdt_cell_t) != 0)
			(*data)++;

		valid = 1;
		while (valid) {
			switch (LIBFDT_DATA32(*data)) {
			case FDT_PROP:
				*data += sizeof(fdt_cell_t);
				len = LIBFDT_DATA32(*data);
				*data += sizeof(fdt_cell_t);
				*data += sizeof(fdt_cell_t);
				*data += len;
				while ((u64) (*data) % sizeof(fdt_cell_t) != 0)
					(*data)++;
				break;
			case FDT_NOP:
				*data += sizeof(fdt_cell_t);
				break;
			case FDT_BEGIN_NODE:
				*data += sizeof(fdt_cell_t);
				len = strlen(*data);
				*data += len + 1;
				while ((u64) (*data) % sizeof(fdt_cell_t) != 0)
					(*data)++;
				valid++;
				break;
			case FDT_END_NODE:
				*data += sizeof(fdt_cell_t);
				valid--;
				break;
			default:
				return NULL;
				break;
			};
		}
	}

	return NULL;
}

struct fdt_node_header *libfdt_find_node(struct fdt_fileinfo *fdt,
					 const char *node_path)
{
	char *data = NULL;

	if (!fdt || !node_path)
		return NULL;

	data = fdt->data;
	return libfdt_find_node_recursive(&data, fdt->str, node_path);
}

u32 libfdt_reserve_count(struct fdt_fileinfo *fdt)
{
	if (!fdt)
		return 0;

	return fdt->mem_rsvcnt;
}

int libfdt_reserve_address(struct fdt_fileinfo *fdt, u32 index, u64 *addr)
{
	struct fdt_reserve_entry *rsv;

	if (!fdt || !fdt->mem_rsvmap || !addr)
		return -EINVAL;
	if (fdt->mem_rsvcnt <= index)
		return -EINVAL;

	rsv = (struct fdt_reserve_entry *)fdt->mem_rsvmap;
	*addr = LIBFDT_DATA64(&rsv[index].address);

	return 0;
}

int libfdt_reserve_size(struct fdt_fileinfo *fdt, u32 index, u64 *size)
{
	struct fdt_reserve_entry *rsv;

	if (!fdt || !fdt->mem_rsvmap || !size)
		return -EINVAL;
	if (fdt->mem_rsvcnt <= index)
		return -EINVAL;

	rsv = (struct fdt_reserve_entry *)fdt->mem_rsvmap;
	*size = LIBFDT_DATA64(&rsv[index].size);

	return 0;
}

/**
 * 获取指定name的property 指针
 * 此接口用于直接从原始fdt内存单元中解析获取属性值(大端格式)
 */
void *libfdt_get_property(struct fdt_fileinfo *fdt,
			struct fdt_node_header *fdt_node,
			const char *name, u32 *size)
{
	u32 len = 0;
	struct fdt_property *prop = NULL;
	char *data = NULL;

	if (!fdt || !fdt_node || !name) {
		return NULL;
	}

	if (LIBFDT_DATA32(&fdt_node->tag) != FDT_BEGIN_NODE)
		return NULL;

	data = (char *)fdt_node;
	data += sizeof(fdt_cell_t);

	len = strlen(data);
	data += len + 1;
	while ((u64) (data) % sizeof(fdt_cell_t) != 0)
		data++;

	while (LIBFDT_DATA32(data) == FDT_PROP) {
		data += sizeof(fdt_cell_t);
		len = LIBFDT_DATA32(data);
		data += sizeof(fdt_cell_t);
		if (!strcmp(&fdt->str[LIBFDT_DATA32(data)], name)) {
			data -= sizeof(fdt_cell_t) * 2;
			prop = (struct fdt_property *)data;
			break;
		}
		data += sizeof(fdt_cell_t);
		data += len;
		while ((u64) (data) % sizeof(fdt_cell_t) != 0)
			(data)++;
	}

	if (!prop)
		return NULL;

	if (size)
		*size = len;

	return prop->data;
}

/**
 * 填充得到root device_node
 */
int libfdt_populate_devtree(struct fdt_fileinfo *fdt,
			 struct device_node **root,
			 const char *root_name,
			 struct device_node *root_parent)
{
	char *data;
	struct device_node *node;

	if (!fdt || !root_name)
		return -EINVAL;

	data = fdt->data;

	if (LIBFDT_DATA32(data) != FDT_BEGIN_NODE)
		return -EINVAL;

	data += sizeof(fdt_cell_t);
	node = dt_node_add(root_parent, root_name);
	if (!node)
		return -ENOMEM;
	if (root)
		*root = node;

	data += strlen(data) + 1;
	while ((u64) (data) % sizeof(fdt_cell_t) != 0)
		(data)++;
	libfdt_populate_devtree_recursive(fdt, node, &data);

	return 0;
}


